#include "debugwindow.h"
#include "ui_debugwindow.h"
#include "mainwindow.h"
#include <QMessageBox>
#include <QCloseEvent>
#include <QBitmap>
#include "imagetostr.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include <sstream>

using namespace std;
DebugWindow::DebugWindow(MainWindow *mainWindow, int ID)://, NetworkWrapper& wrapy) : net(wrapy),
    // Edited because if it is child it places itself on top always
    QMainWindow(0),

    ui(new Ui::DebugWindow),
    mainWindowP(mainWindow)
{
    ui->setupUi(this);
    this->botnum = ID;
    QImage rdrImage("../SwarmGUI/Resource/radar-md.png");
    rdrImage = rdrImage.scaled(201, 191, Qt::IgnoreAspectRatio);
    ui->radarImage->setPixmap(QPixmap::fromImage(rdrImage));

    //on_btnDebugInfo_clicked(); // get debug info
}

void DebugWindow::closeEvent(QCloseEvent *event){

    this->mainWindowP->writeMessage(botnum, SwarmOS::OS_INSTRUCTION + SwarmOS::RESUME);

    if( MainWindow * p = dynamic_cast< MainWindow* >( mainWindowP ) ){
        p->closeDebugWindow(this->botnum);
        event->accept();
    }
    else{
        QMessageBox msg;
        msg.setInformativeText("Something bad happened");
        msg.setModal(true);
        msg.exec();
    }
}

DebugWindow::~DebugWindow()
{
    delete ui;
}

void DebugWindow::on_exitButton_clicked()
{
    string resumeCountStr = ui->resumeCount->text().toStdString();
    if(resumeCountStr == "" || atoi(resumeCountStr.c_str()) <= 0)
    {
        close();
    }
    else
    {
        this->mainWindowP->writeMessage(botnum, SwarmOS::OS_INSTRUCTION + SwarmOS::RESUME + " " + (resumeCountStr));
        waitAndProcessReturns();
    }
}

void DebugWindow::on_sendCommandButton_clicked()
{
    string text = ui->commandEdit->text().toUpper().toStdString();
    mainWindowP->writeMessage(botnum, text);
    if (text.find(SwarmOS::OS_INSTRUCTION) == 0)
    {
        waitAndProcessReturns();
        on_btnDebugInfo_clicked(); // get new debug info
    }
}

QImage DebugWindow::putImage(const cv::Mat& mat)
{
    // 8-bits unsigned, NO. OF CHANNELS=1
    if(mat.type()==CV_8UC1)
    {
        // Set the color table (used to translate colour indexes to qRgb values)
        QVector<QRgb> colorTable;
        for (int i=0; i<256; i++)
            colorTable.push_back(qRgb(i,i,i));
        // Copy input Mat
        const uchar *qImageBuffer = (const uchar*)mat.data;
        // Create QImage with same dimensions as input Mat
        QImage img(qImageBuffer, mat.cols, mat.rows, mat.step, QImage::Format_Indexed8);
        img.setColorTable(colorTable);
        return img;
    }
    // 8-bits unsigned, NO. OF CHANNELS=3
    if(mat.type()==CV_8UC3)
    {
        // Copy input Mat
        const uchar *qImageBuffer = (const uchar*)mat.data;
        // Create QImage with same dimensions as input Mat
        QImage img(qImageBuffer, mat.cols, mat.rows, mat.step, QImage::Format_RGB888);
        return img.rgbSwapped();
    }
    else
    {
        return QImage();
    }
}

QImage addColorSquare(QImage rdrImage, int x, int y, int botRGB,int botColorRadius)
{
    for(int j = 0; j < botColorRadius; j++)//y + or - j
    {
        for(int k = 0; k < botColorRadius; k++ )//x + or - k
        {
        //rdrImage.setPixel(x - j, y, botRGB);
        //rdrImage.setPixel(x + j, y, botRGB);
        //rdrImage.setPixel(x, y - j, botRGB);
        //rdrImage.setPixel(x, y + j, botRGB);
            if(rdrImage.valid(rdrImage.width()/2 + x - k,   rdrImage.height()/2 + y - j))
            {
                rdrImage.setPixel(rdrImage.width()/2 + x - k, rdrImage.height()/2 + y - j, botRGB);
            }
            if(rdrImage.valid(rdrImage.width()/2 + x + k,   rdrImage.height()/2 + y - j))
            {
                rdrImage.setPixel(rdrImage.width()/2 + x + k, rdrImage.height()/2 + y - j, botRGB);
            }
            if(rdrImage.valid(rdrImage.width()/2 + x - k,   rdrImage.height()/2 + y + j))
            {
                rdrImage.setPixel(rdrImage.width()/2 + x - k, rdrImage.height()/2 + y + j, botRGB);
            }
            if(rdrImage.valid(rdrImage.width()/2 + x + k,   rdrImage.height()/2 + y + j))
            {
                rdrImage.setPixel(rdrImage.width()/2 + x + k, rdrImage.height()/2 + y + j, botRGB);
            }
           /* cout << "x " << x << ", " << "y " << y << " " << boolalpha
                 << rdrImage.valid(rdrImage.width()/2 + x - k,   rdrImage.height()/2 + y - j) << " "
                 << rdrImage.valid(rdrImage.width()/2 + x + k,   rdrImage.height()/2 + y - j) << " "
                 << rdrImage.valid(rdrImage.width()/2 + x - k,   rdrImage.height()/2 + y + j) << " "
                 << rdrImage.valid(rdrImage.width()/2 + x + k,   rdrImage.height()/2 + y + j) << endl;*/
        }
    }
    return rdrImage;
}

void DebugWindow::on_btnDebugInfo_clicked()
{
    int nbytes = this->mainWindowP->writeMessage(botnum, SwarmOS::OS_INSTRUCTION + SwarmOS::DEBUG);
    if( nbytes < 0)
    {
        ui->txtDebugOutput->append(QString::fromStdString("unresponsive\nerror number "  + itoa(nbytes)));
        return;
    }

    waitAndProcessReturns();
}

void DebugWindow::on_btnDebugImages_clicked()
{
    if( this->mainWindowP->writeMessage(botnum, SwarmOS::OS_INSTRUCTION + SwarmOS::DEBUGIMG) < 0)
    {
        ui->txtDebugOutput->append("unresponsive");
        return;
    }

    // assert debug image header
    time_t start, now;
    time(&start);
    while (messageQueue.isEmpty())
    {
        time(&now);
        if(difftime(now, start) > 15)
        {
            ui->txtDebugOutput->append(QString::fromStdString("unresponsive (no DEBUG IMG header)\nerror time out"));
            return;
        }
    }
    while (messageQueue.front()[0] != SwarmOS::DEBUG_IMG_HDR)
    {
        while (messageQueue.isEmpty())
        {
            time(&now);
            if(difftime(now, start) > 15)
            {
                ui->txtDebugOutput->append(QString::fromStdString("unresponsive (no DEBUG IMG header)\nerror time out"));
                return;
            }
        }
    }
    messageQueue.pop_front();

    vector<list <string> > imList(4);
    int imageCount = 0;
    int packetCount = 0;
    time(&start);
    while(imageCount < 4)
    {
        time(&now);
        if(difftime(now, start) > 15)
        {
            ui->txtDebugOutput->append(QString::fromStdString("unresponsive\nerror time out"));
            return;
        }

        if(messageQueue.size() > 0 && imageCount < 4)
        {
            if(packetCount > 0)//in the middle of a image
            {
                imList[imageCount].push_back(messageQueue.front()[0]);
                messageQueue.pop_front();
                packetCount--;
                if(packetCount == 0)
                {
                    imageCount++;
                }
                continue;
            }
            else if(messageQueue.front()[0].substr(0, 5) == "IMAGE")//if beginning of image
            {
                imList[imageCount].push_back(messageQueue.front()[0]);
                sscanf(messageQueue.front()[0].c_str(), "%*s %*i %*i %*s %i", &packetCount);
                messageQueue.pop_front();
                if(packetCount == 0)
                {
                    imageCount++;
                }
            }
            else// garbage
            {
                messageQueue.pop_front();
            }
        }

    }

    // remove DONE signal
    while (messageQueue.isEmpty());
    messageQueue.pop_front();

    //update camera images
    ui->image0->setPixmap(QPixmap::fromImage(this->putImage(strToCvMat(imList[0]))));
    ui->image1->setPixmap(QPixmap::fromImage(this->putImage(strToCvMat(imList[1]))));
    ui->image2->setPixmap(QPixmap::fromImage(this->putImage(strToCvMat(imList[2]))));
    ui->image3->setPixmap(QPixmap::fromImage(this->putImage(strToCvMat(imList[3]))));
}
void DebugWindow::on_commandEdit_returnPressed()
{
    on_sendCommandButton_clicked();
}
void DebugWindow::on_ClearAllButton_clicked()
{
    ui->txtDebugOutput->clear();
    ui->lblBumper->clear();
    ui->lblLED->clear();
    ui->lblLeftMotor->clear();
    ui->lblRightMotor->clear();
    ui->IPTextEdit->clear();
    ui->conTextEdit->clear();
}
string btoa(bool b)
{
    return b ? "TRUE":"FALSE";
}

void DebugWindow::on_SaveImageButton_clicked()
{
    this->botnum;
    time_t now;
    time(&now);
    struct tm* timeinfo= localtime(&now);
    char buffer[200];
    strftime (buffer,100,"%m:%d:%y:%T.",timeinfo);
    bool isSaved = ui->image0->pixmap()->toImage().save(QString::fromStdString("../SwarmGUI/Resource/Pics/robot"+itoa(botnum)+"time"+buffer+"full.jpg"));
    isSaved = ui->image1->pixmap()->toImage().save(QString::fromStdString("../SwarmGUI/Resource/Pics/robot"+itoa(botnum)+"time"+buffer+"red.jpg"));
    isSaved = ui->image2->pixmap()->toImage().save(QString::fromStdString("../SwarmGUI/Resource/Pics/robot"+itoa(botnum)+"time"+buffer+"green.jpg"));
    isSaved = ui->image3->pixmap()->toImage().save(QString::fromStdString("../SwarmGUI/Resource/Pics/robot"+itoa(botnum)+"time"+buffer+"blue.jpg"));
    ui->txtDebugOutput->append(QString::fromStdString(btoa(isSaved)));
    //isSaved = ui->image0->pixmap()->toImage().save(QString::fromStdString("full.jpg"));
   // ui->txtDebugOutput->append(QString::fromStdString(btoa(isSaved)));
}

void DebugWindow::addMessage(vector<string> message)
{
    ui->txtDebugOutput->append(QString::fromStdString(message[0]));
}

void DebugWindow::waitAndProcessReturns()
{
    time_t start, now;
    time(&start);
    list<vector<string> > tempMessages;
    while(true)
    {
        time(&now);
        if(!messageQueue.isEmpty())
        {
            if (messageQueue.front()[0] != SwarmOS::NET_DATA_DONE)
            {
                tempMessages.push_back(messageQueue.pop_front()); // save messages
            }
            else
            {
                // got the DONE signal
                messageQueue.pop_front(); // discard it

                // empty tempMessages back whence they came
                // messageQueue should be empty; OS will not send anything, is waiting for command
                while (!tempMessages.empty())
                {
                    messageQueue.push_back(tempMessages.front());
                    tempMessages.pop_front();
                }
                break;
            }
        }
        if(difftime(now, start) > 15)
        {
            ui->txtDebugOutput->append(QString::fromStdString("waitAndProcessReturns(): timed out waiting for SwarmOS::NET_DATA_DONE"));
            return;
        }
    }

    while (!messageQueue.isEmpty())
    {
        while (!messageQueue.isEmpty() && !messageQueue.front()[0].find(SwarmOS::SWARM_DATA) != 0)
        {
            messageQueue.pop_front();
        }

        if (!messageQueue.isEmpty())
        {
            string dataType = messageQueue.pop_front()[0];
            if (dataType.find(SwarmOS::DEBUG_HDR) == 0)
            {
                processDebugInfo();
            }
        }
    }
}

inline void DebugWindow::processDebugInfo()
{
    vector<  vector< string > > ms;
    int i = 0;
    time_t start, now;
    time(&start);
    while(i < 3)
    {
        time(&now);
        if (!messageQueue.isEmpty()) {
            ms.push_back(messageQueue.front());
            //ui->txtDebugOutput->append(QString::fromStdString(messageQueue.front()[0]));
            messageQueue.pop_front();
            i++;
        }
        else if(difftime(now, start) > 15)
        {
            ui->txtDebugOutput->append(QString::fromStdString("unresponsive\nerror time out"));
            ui->txtDebugOutput->append(QString::fromStdString(itoa(difftime(now, start))));
            return;
        }
    }
    //ui->txtDebugOutput->append(QString::fromStdString(ms[0][0] + "/n" + ms[1][0] + "/n" + ms[0][1] + "/n" + ms[0][2]));
    QPalette image0Palette, image1Palette, image2Palette, image3Palette, radarPalette;
    //magic code ploting positions on a palette
    //magic code transforming strings to images

    string periphs = ms[0][0];

    // init skips past space after "PERIPHERALS: " header; loop parses space-separated list thereafter
    // list is in doubles of peripheral name and value
    // ( i > 0: find returns -1 at the end.  i is set to find + 1.)
    string color = "";
    string bump = "";
    for(int i = periphs.find(" ") + 1; i > 0; i = periphs.find(" ", i) + 1)
    {
        string name = periphs.substr(i, periphs.find(" ", i) - i);
        i = periphs.find(" ", i) + 1;
        string value = periphs.substr(i, periphs.find(" ", i) - i);
        if (name == "RED") {
            if (value == "ON") {
                color += "R";
            }
        }
        else if (name == "BLUE") {
            if (value == "ON") {
                color += "B";
            }
        }
        else if (name == "GREEN") {
            if (value == "ON") {
                color += "G";
            }
        }
        else if (name == "RIGHT") {
            ui->lblRightMotor->setText(QString::fromStdString(value));
        }
        else if (name == "LEFT") {
            ui->lblLeftMotor->setText(QString::fromStdString(value));
        }
        else if (name == "FRONT") {
            if (value == "ON") {
                bump += "Front ";
            }
        }
        else if (name == "BACK") {
            if (value == "ON") {
                color += "Back ";
            }
        }
    }
    ui->lblLED->setText(QString::fromStdString(color));
    ui->lblBumper->setText(QString::fromStdString(bump));

    string finds = ms[1][0];
    vector<string> locs;

    // init skips past space after "ROBOTS: " header; loop parses space-separated list thereafter
    // list is in triplets of robot radius, angle, and color
    stringstream ss(finds);
    ss >> finds;
    ss >> finds;
    while(!ss.eof())
    {
        locs.push_back(finds);
        //ui->txtDebugOutput->append(QString::fromStdString(finds + "#/n"));
        ss >> finds;
    }
    int x;
    int y;
    int botColorRadius = 12;
    string botColor;
    unsigned int botRGB;
    QImage rdrImage("../SwarmGUI/Resource/radar-md.png");//will probably load a radar like image here
    rdrImage = rdrImage.scaled(201, 191, Qt::IgnoreAspectRatio);
    if(rdrImage.isNull())
    {
        ui->txtDebugOutput->append(QString::fromStdString("NULL image"));
    }
    else
    {
        ui->txtDebugOutput->append(QString::fromStdString("Real image " + itoa(rdrImage.height()) + " " + itoa(rdrImage.width())));
    }

    for(unsigned int i = 0; i < locs.size(); i += 3)
    {
       x = atof(locs[i].c_str()) * sin(- atof(locs[i+1].c_str()));//*pow(10, widthPower);//x = radius*cos(angle) the thousand is to scale it and it might be too much
       y = - atof(locs[i].c_str()) * cos(atof(locs[i+1].c_str()));//*pow(10, heightPower);//y = radius*sin(angle)
       botColor = locs[i+2];

       if(botColor.find("BLUE") != string::npos)
       {
           botRGB = 0xff0000ff;//fully opaque no red no green fully blue
       }
       if(botColor.find("GREEN") != string::npos)
       {
           botRGB = 0xff00ff00;//fully opaque no redfully green no blue
       }
       if(botColor.find("RED") != string::npos)
       {
           botRGB = 0xffff0000;//fully opaque fully red no green no blue
       }
       rdrImage = addColorSquare(rdrImage, x, y, botRGB, botColorRadius);
    }
    ui->radarImage->setPixmap(QPixmap::fromImage(rdrImage));//update radar image
    //end magic
    //network debug info
    stringstream botnet(ms[2][0]);

    botnet >> finds;//get rid of header
    vector <string> botIPs, botCons;
    while(!botnet.eof())
    {
        botnet >> finds;
        botIPs.push_back(finds);

        if(!botnet.eof())
        {
            botnet >> finds;
            botCons.push_back(finds);
        }

    }
    ui->IPTextEdit->clear();
    ui->conTextEdit->clear();
    for(unsigned int i = 0; i < botCons.size(); i++)
    {
        ui->IPTextEdit->append(QString::fromStdString(botIPs[i] + "\n"));
        ui->conTextEdit->append(QString::fromStdString(botCons[i] + "\n"));
    }
    //end network debug info
}
